#include <tf/transform_listener.h>
#include "MainApp.h"
#include "PatrolPlanner.h"
#include "Patroller.h"

/**
 * Parses node arguments
 */
MainApp::MainApp() {

	// parse node arguments
	m_node.getParam(X_LOCS_PARAM, m_xLocs);
	m_node.getParam(Y_LOCS_PARAM, m_yLocs);
	m_node.getParam(GOAL_TOLERANCE, m_goalTolerance);
	m_node.getParam(WAIT_AT_GOAL, m_waitAtGoal);
	m_node.getParam(NUM_LAPS, m_numLaps);
	m_node.getParam(LOG_FILE_NAME, m_logFileName);
	m_node.getParam(WAIT_FOR_SERVER, m_waitForServer);
}

/**
 * Destructor
 */
MainApp::~MainApp() { }

/**
 * Allocates necessary objects and uses them to run the patrol
 */
void MainApp::runTask() {

	// get robot's current position
	tf::TransformListener listener;
	listener.waitForTransform("/map", "/base_footprint", ros::Time(0),
				ros::Duration(10.0));
	tf::StampedTransform transform;
	geometry_msgs::Point initPos;
	try {
		listener.lookupTransform("/map", "/base_footprint", ros::Time(0),
				transform);

		initPos.x = transform.getOrigin().x();
		initPos.y = transform.getOrigin().y();
		initPos.z = transform.getOrigin().z();

		// display initial position
		ROS_INFO_STREAM("Initial position: (" << initPos.x << "," << initPos.y << "," << initPos.z << ")");

	} catch (tf::TransformException &ex) {
		ROS_ERROR("%s", ex.what());
		return;
	}

	// create planner
	PatrolPlanner planner(m_node, m_xLocs, m_yLocs, m_goalTolerance);

	// plan patrol path
	std::vector<geometry_msgs::Point> patrolPath;
	std::vector<float> segmentsCost;
	float initCost;
	planner.planPatrolPath(initPos, patrolPath, segmentsCost, initCost);

	// display planned patrol path
	ROS_INFO("in MainApp.MainApp(): patrol path:");
	for (int i = 0; i < patrolPath.size(); ++i) {
		ROS_INFO_STREAM("(" << patrolPath[i].x << "," << patrolPath[i].y << "," << patrolPath[i].z << ")");
	}

	// run patrol
	Patroller patroller(m_waitAtGoal, m_numLaps, m_logFileName, m_waitForServer);
	patroller.runPatrol(patrolPath, initCost, segmentsCost);
}
